/*********************************************************************
 This is a library for our Monochrome OLEDs based on SSD1306 drivers

 Pick one up today in the adafruit shop!
 ------> http://www.adafruit.com/category/63_98

 These displays use SPI to communicate, 4 or 5 pins are required to
 interface

 Adafruit invests time and resources providing this open source code,
 please support Adafruit and open-source hardware by purchasing
 products from Adafruit!

 Written by Limor Fried/Ladyada  for Adafruit Industries.
 BSD license, check license.txt for more information
 All text above, and the splash screen below must be included in any redistribution
 *********************************************************************/

#include <avr/pgmspace.h>
#include <util/delay.h>
#include <stdlib.h>

#include <Wire.h>

#include "Adafruit_GFX.h"
#include "Adafruit_SSD1306.h"

#include "glcdfont.c"

// a 5x7 font table
extern uint8_t PROGMEM font[];

extern uint8_t led;

// the memory buffer for the LCD

static uint8_t buffer[SSD1306_LCDHEIGHT * SSD1306_LCDWIDTH / 8] =
{ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x80, 0x80, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x80, 0xC0, 0xC0,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x80, 0xC0, 0xE0, 0xF0, 0xF8, 0xFC, 0xF8, 0xE0, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x80, 0x80, 0x80, 0x80, 0x80, 0x00, 0x80, 0x80,
		0x00, 0x00, 0x00, 0x00, 0x80, 0x80, 0x80, 0x80, 0x80, 0x00, 0xFF, 0xFF,
		0xFF, 0x00, 0x00, 0x00, 0x00, 0x80, 0x80, 0x80, 0x80, 0x00, 0x00, 0x80,
		0x80, 0x00, 0x00, 0x80, 0xFF, 0xFF, 0x80, 0x80, 0x00, 0x80, 0x80, 0x00,
		0x80, 0x80, 0x80, 0x80, 0x00, 0x80, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x80, 0x80, 0x00, 0x00, 0x8C, 0x8E, 0x84, 0x00, 0x00, 0x80, 0xF8, 0xF8,
		0xF8, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0,
		0xF0, 0xF0, 0xF0, 0xE0, 0xE0, 0xC0, 0x80, 0x00, 0xE0, 0xFC, 0xFE, 0xFF,
		0xFF, 0xFF, 0x7F, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFE, 0xFF,
		0xC7, 0x01, 0x01, 0x01, 0x01, 0x83, 0xFF, 0xFF, 0x00, 0x00, 0x7C, 0xFE,
		0xC7, 0x01, 0x01, 0x01, 0x01, 0x83, 0xFF, 0xFF, 0xFF, 0x00, 0x38, 0xFE,
		0xC7, 0x83, 0x01, 0x01, 0x01, 0x83, 0xC7, 0xFF, 0xFF, 0x00, 0x00, 0x01,
		0xFF, 0xFF, 0x01, 0x01, 0x00, 0xFF, 0xFF, 0x07, 0x01, 0x01, 0x01, 0x00,
		0x00, 0x7F, 0xFF, 0x80, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0x7F, 0x00, 0x00,
		0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x01, 0xFF, 0xFF, 0xFF, 0x01, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03,
		0x0F, 0x3F, 0x7F, 0x7F, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xE7,
		0xC7, 0xC7, 0x8F, 0x8F, 0x9F, 0xBF, 0xFF, 0xFF, 0xC3, 0xC0, 0xF0, 0xFF,
		0xFF, 0xFF, 0xFF, 0xFF, 0xFC, 0xFC, 0xFC, 0xFC, 0xFC, 0xFC, 0xFC, 0xFC,
		0xF8, 0xF8, 0xF0, 0xF0, 0xE0, 0xC0, 0x00, 0x01, 0x03, 0x03, 0x03, 0x03,
		0x03, 0x01, 0x03, 0x03, 0x00, 0x00, 0x00, 0x00, 0x01, 0x03, 0x03, 0x03,
		0x03, 0x01, 0x01, 0x03, 0x01, 0x00, 0x00, 0x00, 0x01, 0x03, 0x03, 0x03,
		0x03, 0x01, 0x01, 0x03, 0x03, 0x00, 0x00, 0x00, 0x03, 0x03, 0x00, 0x00,
		0x00, 0x03, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x03,
		0x03, 0x03, 0x03, 0x03, 0x01, 0x00, 0x00, 0x00, 0x01, 0x03, 0x01, 0x00,
		0x00, 0x00, 0x03, 0x03, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
#if (SSD1306_LCDHEIGHT == 64)
		0x00, 0x00, 0x00, 0x80, 0xC0, 0xE0, 0xF0, 0xF9, 0xFF, 0xFF, 0xFF, 0xFF,
		0xFF, 0x3F, 0x1F, 0x0F, 0x87, 0xC7, 0xF7, 0xFF, 0xFF, 0x1F, 0x1F, 0x3D,
		0xFC, 0xF8, 0xF8, 0xF8, 0xF8, 0x7C, 0x7D, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
		0xFF, 0xFF, 0xFF, 0x7F, 0x3F, 0x0F, 0x07, 0x00, 0x30, 0x30, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFE, 0xFE, 0xFC, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xE0, 0xC0, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x30, 0x30,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xC0, 0xFE, 0xFF,
		0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x7F, 0x7F, 0x3F, 0x1F,
		0x0F, 0x07, 0x1F, 0x7F, 0xFF, 0xFF, 0xF8, 0xF8, 0xFF, 0xFF, 0xFF, 0xFF,
		0xFF, 0xFE, 0xF8, 0xE0, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0xFE, 0xFE, 0x00, 0x00, 0x00, 0xFC, 0xFE, 0xFC,
		0x0C, 0x06, 0x06, 0x0E, 0xFC, 0xF8, 0x00, 0x00, 0xF0, 0xF8, 0x1C, 0x0E,
		0x06, 0x06, 0x06, 0x0C, 0xFF, 0xFF, 0xFF, 0x00, 0x00, 0xFE, 0xFE, 0x00,
		0x00, 0x00, 0x00, 0xFC, 0xFE, 0xFC, 0x00, 0x18, 0x3C, 0x7E, 0x66, 0xE6,
		0xCE, 0x84, 0x00, 0x00, 0x06, 0xFF, 0xFF, 0x06, 0x06, 0xFC, 0xFE, 0xFC,
		0x0C, 0x06, 0x06, 0x06, 0x00, 0x00, 0xFE, 0xFE, 0x00, 0x00, 0xC0, 0xF8,
		0xFC, 0x4E, 0x46, 0x46, 0x46, 0x4E, 0x7C, 0x78, 0x40, 0x18, 0x3C, 0x76,
		0xE6, 0xCE, 0xCC, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x01, 0x07, 0x0F, 0x1F, 0x1F, 0x3F, 0x3F, 0x3F, 0x3F, 0x1F, 0x0F, 0x03,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x0F, 0x0F, 0x00, 0x00, 0x00, 0x0F, 0x0F, 0x0F, 0x00, 0x00, 0x00, 0x00,
		0x0F, 0x0F, 0x00, 0x00, 0x03, 0x07, 0x0E, 0x0C, 0x18, 0x18, 0x0C, 0x06,
		0x0F, 0x0F, 0x0F, 0x00, 0x00, 0x01, 0x0F, 0x0E, 0x0C, 0x18, 0x0C, 0x0F,
		0x07, 0x01, 0x00, 0x04, 0x0E, 0x0C, 0x18, 0x0C, 0x0F, 0x07, 0x00, 0x00,
		0x00, 0x0F, 0x0F, 0x00, 0x00, 0x0F, 0x0F, 0x0F, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x0F, 0x0F, 0x00, 0x00, 0x00, 0x07, 0x07, 0x0C, 0x0C, 0x18,
		0x1C, 0x0C, 0x06, 0x06, 0x00, 0x04, 0x0E, 0x0C, 0x18, 0x0C, 0x0F, 0x07,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
#endif
		};

// the most basic function, set a single pixel
void Adafruit_SSD1306::drawPixel(int16_t x, int16_t y, uint16_t color)
{
	if ((x < 0) || (x >= width()) || (y < 0) || (y >= height()))
		return;

	// check rotation, move pixel around if necessary
	switch (getRotation())
	{
	case 1:
		swap(x, y);
		x = WIDTH - x - 1;
		break;
	case 2:
		x = WIDTH - x - 1;
		y = HEIGHT - y - 1;
		break;
	case 3:
		swap(x, y);
		y = HEIGHT - y - 1;
		break;
	}

	// x is which column
	if (color == WHITE)
		buffer[x + (y / 8) * SSD1306_LCDWIDTH] |= _BV((y%8));
	else
		buffer[x + (y / 8) * SSD1306_LCDWIDTH] &= ~_BV((y%8));
}

Adafruit_SSD1306::Adafruit_SSD1306(int8_t SID, int8_t SCLK, int8_t DC,
		int8_t RST, int8_t CS)
{
	cs = CS;
	rst = RST;
	dc = DC;
	sclk = SCLK;
	sid = SID;
}

// initializer for I2C - we only indicate the reset pin!
Adafruit_SSD1306::Adafruit_SSD1306(int8_t reset)
{
	sclk = dc = cs = sid = -1;
	rst = reset;
}

void Adafruit_SSD1306::begin(uint8_t vccstate, uint8_t i2caddr)
{
#ifdef SSD1306_128_64
	constructor(128, 64);
#endif
#ifdef SSD1306_128_32
	constructor(128, 32);
#endif

	_i2caddr = i2caddr;

	// set pin directions
	if (sid != -1)
	{
		// SPI
		pinMode(sid, OUTPUT);
		pinMode(sclk, OUTPUT);
		pinMode(dc, OUTPUT);
		pinMode(cs, OUTPUT);
		clkport = portOutputRegister(digitalPinToPort(sclk));
		clkpinmask = digitalPinToBitMask(sclk);
		mosiport = portOutputRegister(digitalPinToPort(sid));
		mosipinmask = digitalPinToBitMask(sid);
		csport = portOutputRegister(digitalPinToPort(cs));
		cspinmask = digitalPinToBitMask(cs);
		dcport = portOutputRegister(digitalPinToPort(dc));
		dcpinmask = digitalPinToBitMask(dc);
	}
	else
	{
		// I2C Init
		Wire.begin(); // Is this the right place for this?
	}

	// Setup reset pin direction (used by both SPI and I2C)
	pinMode(rst, OUTPUT);
	digitalWrite(rst, HIGH);
	// VDD (3.3V) goes high at start, lets just chill for a ms
	delay(1);
	// bring reset low
	digitalWrite(rst, LOW);
	// wait 10ms
	delay(10);
	// bring out of reset
	digitalWrite(rst, HIGH);
	// turn on VCC (9V?)

#if defined SSD1306_128_32
	// Init sequence for 128x32 OLED module
	ssd1306_command(SSD1306_DISPLAYOFF); // 0xAE
	ssd1306_command(SSD1306_SETDISPLAYCLOCKDIV); // 0xD5
	ssd1306_command(0x80); // the suggested ratio 0x80
	ssd1306_command(SSD1306_SETMULTIPLEX); // 0xA8
	ssd1306_command(0x1F);
	ssd1306_command(SSD1306_SETDISPLAYOFFSET); // 0xD3
	ssd1306_command(0x0); // no offset
	ssd1306_command(SSD1306_SETSTARTLINE | 0x0); // line #0
	ssd1306_command(SSD1306_CHARGEPUMP); // 0x8D
	if (vccstate == SSD1306_EXTERNALVCC)
	{
		ssd1306_command(0x10);
	}
	else
	{
		ssd1306_command(0x14);
	}
	ssd1306_command(SSD1306_MEMORYMODE); // 0x20
	ssd1306_command(0x00); // 0x0 act like ks0108
	ssd1306_command(SSD1306_SEGREMAP | 0x1);
	ssd1306_command(SSD1306_COMSCANDEC);
	ssd1306_command(SSD1306_SETCOMPINS); // 0xDA
	ssd1306_command(0x02);
	ssd1306_command(SSD1306_SETCONTRAST); // 0x81
	ssd1306_command(0x8F);
	ssd1306_command(SSD1306_SETPRECHARGE); // 0xd9
	if (vccstate == SSD1306_EXTERNALVCC)
	{
		ssd1306_command(0x22);
	}
	else
	{
		ssd1306_command(0xF1);
	}
	ssd1306_command(SSD1306_SETVCOMDETECT); // 0xDB
	ssd1306_command(0x02);
	ssd1306_command(SSD1306_DISPLAYALLON_RESUME); // 0xA4
	ssd1306_command(SSD1306_NORMALDISPLAY); // 0xA6
#endif




#if defined SSD1306_128_64
	// Init sequence for 128x64 OLED module
	ssd1306_command(SSD1306_DISPLAYOFF); // 0xAE
	ssd1306_command(SSD1306_SETDISPLAYCLOCKDIV); // 0xD5
	ssd1306_command(0x80); // the suggested ratio 0x80
	ssd1306_command(SSD1306_SETMULTIPLEX); // 0xA8
	ssd1306_command(0x3F); // set to max (default)
	ssd1306_command(SSD1306_SETDISPLAYOFFSET); // 0xD3
	ssd1306_command( 0x38 ); // no offset
	ssd1306_command(SSD1306_SETSTARTLINE | 0x00); // line #0; TMS: doesn't seem to affect anything//TMS: CHANGED TO 0x02 from 0x40
	ssd1306_command(SSD1306_CHARGEPUMP); // 0x8D
	if (vccstate == SSD1306_EXTERNALVCC)
	{
		ssd1306_command(0x10);
	}
	else
	{
		ssd1306_command(0x14);
	}
	ssd1306_command(SSD1306_MEMORYMODE); // 0x20
	ssd1306_command(0x00); // 0x00 act like ks0108; TMS: Horizontal addressing mode
	ssd1306_command(SSD1306_SEGREMAP | 0x01); // TMS: flips horizontally
	ssd1306_command(SSD1306_COMSCANDEC); // TMS: Flips vertically
	ssd1306_command(SSD1306_SETCOMPINS); // 0xDA
	ssd1306_command(0x12);
	ssd1306_command(SSD1306_SETCONTRAST); // 0x81
	if (vccstate == SSD1306_EXTERNALVCC)
	{
		ssd1306_command(0x9F);
	}
	else
	{
		ssd1306_command(0xCF);
	}
	ssd1306_command(SSD1306_SETPRECHARGE); // 0xd9
	if (vccstate == SSD1306_EXTERNALVCC)
	{
		ssd1306_command(0x22);
	}
	else
	{
		ssd1306_command(0xF1);
	}
	ssd1306_command(SSD1306_SETVCOMDETECT); // 0xDB
	ssd1306_command(0x20); // TMS: missing; set to default

	ssd1306_command(SSD1306_SETSTARTLINE | 0x00); // Set RAM to line 0

	ssd1306_command(SSD1306_DISPLAYALLON_RESUME); // 0xA4

	ssd1306_command(SSD1306_NORMALDISPLAY); // 0xA6
#endif

	ssd1306_command(SSD1306_DISPLAYON);//--turn on oled panel


}

void Adafruit_SSD1306::invertDisplay(uint8_t i)
{
	if (i)
	{
		ssd1306_command(SSD1306_INVERTDISPLAY);
	}
	else
	{
		ssd1306_command(SSD1306_NORMALDISPLAY);
	}
}

void Adafruit_SSD1306::ssd1306_command(uint8_t c)
{
	if (sid != -1)
	{
		// SPI
		//digitalWrite(cs, HIGH);
		*csport |= cspinmask;
		//digitalWrite(dc, LOW);
		*dcport &= ~dcpinmask;
		//digitalWrite(cs, LOW);
		*csport &= ~cspinmask;
		fastSPIwrite(c);
		//digitalWrite(cs, HIGH);
		*csport |= cspinmask;
	}
	else
	{
		// I2C
		uint8_t control = 0x01; // Co = 0, D/C = 0; TMS: Changed to 0x01 from 0x00
		Wire.beginTransmission(_i2caddr);
		Wire.write(control);
		Wire.write(c);
		Wire.endTransmission();
	}
}

// startscrollright
// Activate a right handed scroll for rows start through stop
// Hint, the display is 16 rows tall. To scroll the whole display, run:
// display.scrollright(0x00, 0x0F) 
void Adafruit_SSD1306::startscrollright(uint8_t start, uint8_t stop)
{
	ssd1306_command(SSD1306_RIGHT_HORIZONTAL_SCROLL);
	ssd1306_command(0X00);
	ssd1306_command(start);
	ssd1306_command(0X00);
	ssd1306_command(stop);
	ssd1306_command(0X01);
	ssd1306_command(0XFF);
	ssd1306_command(SSD1306_ACTIVATE_SCROLL);
}

// startscrollleft
// Activate a right handed scroll for rows start through stop
// Hint, the display is 16 rows tall. To scroll the whole display, run:
// display.scrollright(0x00, 0x0F) 
void Adafruit_SSD1306::startscrollleft(uint8_t start, uint8_t stop)
{
	ssd1306_command(SSD1306_LEFT_HORIZONTAL_SCROLL);
	ssd1306_command(0X00);
	ssd1306_command(start);
	ssd1306_command(0X00);
	ssd1306_command(stop);
	ssd1306_command(0X01);
	ssd1306_command(0XFF);
	ssd1306_command(SSD1306_ACTIVATE_SCROLL);
}

// startscrolldiagright
// Activate a diagonal scroll for rows start through stop
// Hint, the display is 16 rows tall. To scroll the whole display, run:
// display.scrollright(0x00, 0x0F) 
void Adafruit_SSD1306::startscrolldiagright(uint8_t start, uint8_t stop)
{
	ssd1306_command(SSD1306_SET_VERTICAL_SCROLL_AREA);
	ssd1306_command(0X00);
	ssd1306_command(SSD1306_LCDHEIGHT);
	ssd1306_command(SSD1306_VERTICAL_AND_RIGHT_HORIZONTAL_SCROLL);
	ssd1306_command(0X00);
	ssd1306_command(start);
	ssd1306_command(0X00);
	ssd1306_command(stop);
	ssd1306_command(0X01);
	ssd1306_command(SSD1306_ACTIVATE_SCROLL);
}

// startscrolldiagleft
// Activate a diagonal scroll for rows start through stop
// Hint, the display is 16 rows tall. To scroll the whole display, run:
// display.scrollright(0x00, 0x0F) 
void Adafruit_SSD1306::startscrolldiagleft(uint8_t start, uint8_t stop)
{
	ssd1306_command(SSD1306_SET_VERTICAL_SCROLL_AREA);
	ssd1306_command(0X00);
	ssd1306_command(SSD1306_LCDHEIGHT);
	ssd1306_command(SSD1306_VERTICAL_AND_LEFT_HORIZONTAL_SCROLL);
	ssd1306_command(0X00);
	ssd1306_command(start);
	ssd1306_command(0X00);
	ssd1306_command(stop);
	ssd1306_command(0X01);
	ssd1306_command(SSD1306_ACTIVATE_SCROLL);
}

void Adafruit_SSD1306::stopscroll(void)
{
	ssd1306_command(SSD1306_DEACTIVATE_SCROLL);
}

void Adafruit_SSD1306::ssd1306_data(uint8_t c)
{
	if (sid != -1)
	{
		// SPI
		//digitalWrite(cs, HIGH);
		*csport |= cspinmask;
		//digitalWrite(dc, HIGH);
		*dcport |= dcpinmask;
		//digitalWrite(cs, LOW);
		*csport &= ~cspinmask;
		fastSPIwrite(c);
		//digitalWrite(cs, HIGH);
		*csport |= cspinmask;
	}
	else
	{
		// I2C
		uint8_t control = 0x02; // Co = 0, D/C = 1; TMS: CHANGED TO 0x02 from 0x40
		Wire.beginTransmission(_i2caddr);
		Wire.write(control);
		Wire.write(c);
		Wire.endTransmission();
	}
}

void Adafruit_SSD1306::display(void)
{
	ssd1306_command(SSD1306_SETLOWCOLUMN | 0x0); // low col = 0
	ssd1306_command(SSD1306_SETHIGHCOLUMN | 0x0); // hi col = 0
	ssd1306_command(SSD1306_SETSTARTLINE | 0x0); // line #0

	if (sid != -1)
	{
		// SPI
		*csport |= cspinmask;
		*dcport |= dcpinmask;
		*csport &= ~cspinmask;

		for (uint16_t i = 0; i < (SSD1306_LCDWIDTH * SSD1306_LCDHEIGHT / 8); i++)
		{
			fastSPIwrite(buffer[i]);
			//ssd1306_data(buffer[i]);
		}
		// i wonder why we have to do this (check datasheet)
		if (SSD1306_LCDHEIGHT == 32)
		{
			for (uint16_t i = 0; i < (SSD1306_LCDWIDTH * SSD1306_LCDHEIGHT / 8); i++)
			{
				//ssd1306_data(0);
				fastSPIwrite(0);
			}
		}
		*csport |= cspinmask;
	}
	else
	{
		// save I2C bitrate
		uint8_t twbrbackup = TWBR;
		TWBR = 12; // upgrade to 400KHz!

		//Serial.println(TWBR, DEC);
		//Serial.println(TWSR & 0x3, DEC);

		// I2C
		for (uint16_t i = 0; i < (SSD1306_LCDWIDTH * SSD1306_LCDHEIGHT / 8); i++)
		{
			// send a bunch of data in one xmission
			Wire.beginTransmission(_i2caddr);
			Wire.write(0x02); //TMS: CHANGED TO 0x02 from 0x40
			for (uint8_t x = 0; x < 16; x++)
			{
				Wire.write(buffer[i]);
				i++;
			}
			i--;
			Wire.endTransmission();
		}

		// i wonder why we have to do this (check datasheet)
		if (SSD1306_LCDHEIGHT == 32)
		{
			for (uint16_t i = 0; i < (SSD1306_LCDWIDTH * SSD1306_LCDHEIGHT / 8); i++)
			{
				// send a bunch of data in one xmission
				Wire.beginTransmission(_i2caddr);
				Wire.write(0x02); //TMS: CHANGED TO 0x02 from 0x40
				for (uint8_t x = 0; x < 16; x++)
				{
					Wire.write((uint8_t) 0x00);
					i++;
				}
				i--;
				Wire.endTransmission();
			}
		}
		TWBR = twbrbackup;
	}
}

// clear everything
void Adafruit_SSD1306::clearDisplay(void)
{
	memset(buffer, 0, (SSD1306_LCDWIDTH * SSD1306_LCDHEIGHT / 8));
}

inline void Adafruit_SSD1306::fastSPIwrite(uint8_t d)
{

	for (uint8_t bit = 0x80; bit; bit >>= 1)
	{
		*clkport &= ~clkpinmask;
		if (d & bit)
			*mosiport |= mosipinmask;
		else
			*mosiport &= ~mosipinmask;
		*clkport |= clkpinmask;
	}
	//*csport |= cspinmask;
}

inline void Adafruit_SSD1306::slowSPIwrite(uint8_t d)
{
	for (int8_t i = 7; i >= 0; i--)
	{
		digitalWrite(sclk, LOW);
		if (d & _BV(i))
		{
			digitalWrite(sid, HIGH);
		}
		else
		{
			digitalWrite(sid, LOW);
		}
		digitalWrite(sclk, HIGH);
	}
}

